#include "bsp_flash.h"
#include "main.h"
#include "iwdg.h"
#include "usart.h"
#include <string.h>
#include <stdio.h>

#if defined(FLASH_SR_RDERR) 
#define FLASH_ERR_ALL               (FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR | FLASH_FLAG_OPERR | FLASH_FLAG_RDERR)
#else
#define FLASH_ERR_ALL               (FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR | FLASH_FLAG_OPERR)
#endif

#define FMC_PECFG                  (*(volatile uint32_t*)(uint32_t)(0x40023C00U + 0x00000020U))     /*!< FMC page erase configuration register */
#define FMC_PEKEY                  (*(volatile uint32_t*)(uint32_t)(0x40023C00U + 0x00000024U))    /*!< FMC unlock page erase key register */

#define FMC_PE_EN                  (0x1UL << FLASH_CR_LOCK_Pos)             /*!< 0x80000000 */
const uint32_t ADDR_FLASH_SECTOR[ADDR_FLASH_SECTOR_NUM+1] = 
{
    ADDR_FLASH_SECTOR_0,
    ADDR_FLASH_SECTOR_1,
    ADDR_FLASH_SECTOR_2,
    ADDR_FLASH_SECTOR_3,
    ADDR_FLASH_SECTOR_4,
    ADDR_FLASH_SECTOR_5,
    ADDR_FLASH_SECTOR_6,
    ADDR_FLASH_SECTOR_7,
    ADDR_FLASH_SECTOR_8,
    ADDR_FLASH_SECTOR_9,
    ADDR_FLASH_SECTOR_10,
    ADDR_FLASH_SECTOR_11,
    ADDR_FLASH_END,
};
#ifdef BSP_FLASH_UNIT_TEST_EN
static uint8_t flash_write_temp[BSP_FLASH_PAGE_SIZE];
#endif
static uint8_t flash_read_temp[BSP_FLASH_PAGE_SIZE];

static int8_t _bsp_flash_page_erase(uint32_t addr,uint32_t page_num)
{
#ifdef BSP_FLASH_UNIT_TEST_EN
    printf("erase page_num:%ld\r\n",page_num);
#endif

#ifdef BSP_FLASH_CONDITION_TEST
    return BSP_FLASH_ERROR_NULL;
#else
    int8_t ret = BSP_FLASH_ERROR_NULL;
    if(HAL_OK != HAL_FLASH_Unlock())
    {
        return -BSP_FLASH_ERROR_UNLOCK;
    }
	__HAL_FLASH_CLEAR_FLAG(FLASH_ERR_ALL);
    idwg_long_period_set();

    // test ok:32s
    // uint32_t start_tick = HAL_GetTick();
    // HAL_IWDG_Refresh(&hiwdg);
    // uint32_t count = 0;
    // while(1)
    // {
    //     printf("count:%ld,%ld\r\n",count++,HAL_GetTick()-start_tick);
    //     HAL_Delay(1);
    // }

    for(volatile uint32_t i = 0;i < page_num;i++)
    {
        // printf("erase page:%lx,%ld\n",addr + i*BSP_FLASH_PAGE_SIZE,i);
        HAL_IWDG_Refresh(&hiwdg);
        if(HAL_OK != FLASH_WaitForLastOperation(BSP_WAIT_OPERATION_TIME))
        {
            ret = -BSP_FLASH_ERROR_WAIT_OPT;
            break;
        }
         /* unlock page erase operation */
        FMC_PEKEY = BSP_UNLOCK_PE_KEY;
        /* start page erase */
        FMC_PECFG = FMC_PE_EN | (addr + i*BSP_FLASH_PAGE_SIZE);

        FLASH->CR &= ~(0x1FUL << 3U);
        FLASH->CR |= (0X01UL<<1U);
        FLASH->CR |= (0X01UL<<16U);

        /* wait for the FMC ready */
        if(HAL_OK != FLASH_WaitForLastOperation(BSP_WAIT_OPERATION_TIME))
        {
            ret = -BSP_FLASH_ERROR_WAIT_OPT;
            break;
        }

        FMC_PECFG &= ~FMC_PE_EN;
        // FMC_CTL &= ~FMC_CTL_SER;
        FLASH->CR &= ~(0X01UL<<1U);
    }
    HAL_FLASH_Lock();
    MX_IWDG_Init();
    return ret;
#endif
}

static int8_t _bsp_flash_write(uint32_t addr,uint8_t *data,uint32_t len)
{
#ifdef BSP_FLASH_CONDITION_TEST
    return BSP_FLASH_ERROR_NULL;
#else
    int8_t ret = BSP_FLASH_ERROR_NULL;
    if(HAL_OK != HAL_FLASH_Unlock())
    {
        return -BSP_FLASH_ERROR_UNLOCK;
    }
	__HAL_FLASH_CLEAR_FLAG(FLASH_ERR_ALL);
    if(HAL_OK != FLASH_WaitForLastOperation(BSP_WAIT_OPERATION_TIME))
    {
        HAL_FLASH_Lock();
        return -BSP_FLASH_ERROR_WAIT_OPT;
    }
    HAL_IWDG_Refresh(&hiwdg);
    // printf("write flash:%lx,%ld\n",addr,len);
    for(volatile uint32_t i = 0;i < len;i++)
    {
        if(HAL_OK != HAL_FLASH_Program(FLASH_TYPEPROGRAM_FAST,addr+i,*(data + i)))
        {
            ret = -BSP_FLASH_ERROR_WRITE_FAILED;
            break;
        }
    }
    HAL_FLASH_Lock();
    return ret;
#endif
}

int8_t bsp_flash_sector_erase(uint32_t addr,uint32_t len)
{
    if((0 == len)
    || (addr < BSP_FLASH_START))
    {
        return -BSP_FLASH_ERROR_PAREMETER;
    }
    else if(addr+len > ADDR_FLASH_END)
    {
        return -BSP_FLASH_ERROR_OVERRANGE;
    }

    volatile uint8_t sector_start = 0;
    volatile uint8_t sector_end = 0;
    int8_t ret = BSP_FLASH_ERROR_NULL;
    while(sector_start < ADDR_FLASH_SECTOR_NUM)
    {
        if(addr == ADDR_FLASH_SECTOR[sector_start])
        {
            break;
        }
        sector_start++;
    }
    if(sector_start >= ADDR_FLASH_SECTOR_NUM)
    {
        return -BSP_FLASH_ERROR_ERASE_SECTOR_START;
    }
    sector_end = sector_start+1;

    while(sector_end < ADDR_FLASH_SECTOR_NUM+1)
    {
        if((addr+len) > ADDR_FLASH_SECTOR[sector_end-1]
        && (addr+len) <= ADDR_FLASH_SECTOR[sector_end])
        {
            break;
        }
        sector_end++;
    }
    if(sector_end >= ADDR_FLASH_SECTOR_NUM+1)
    {
        return -BSP_FLASH_ERROR_ERASE_SECTOR_END;
    }
#ifdef BSP_FLASH_UNIT_TEST_EN
    printf("sector:%d,%d\r\n",sector_start,sector_end);
#endif

#ifdef BSP_FLASH_CONDITION_TEST
#else
    if(HAL_OK != HAL_FLASH_Unlock())
    {
        return -BSP_FLASH_ERROR_UNLOCK;
    }
	__HAL_FLASH_CLEAR_FLAG(FLASH_ERR_ALL);
    idwg_long_period_set();
    HAL_IWDG_Refresh(&hiwdg);
    if(HAL_OK != FLASH_WaitForLastOperation(BSP_WAIT_OPERATION_TIME))
    {
        HAL_FLASH_Lock();
        return -BSP_FLASH_ERROR_WAIT_OPT;
    }
    // MX_IWDG_DISABLE();
    while(sector_start < sector_end)
    {
        HAL_IWDG_Refresh(&hiwdg);
        FLASH_PageErase(sector_start, 1);           //Pay attention, there's a problem here
        __DSB();
        __ISB();
        if(HAL_OK != FLASH_WaitForLastOperation(BSP_WAIT_SECTOR_ERASE_OPERATION_TIME))
        {
            ret = -BSP_FLASH_ERROR_WAIT_OPT;
            break;
        }
        sector_start++;
    }
    // MX_IWDG_ENABLE();
    MX_IWDG_Init();
    HAL_IWDG_Refresh(&hiwdg);
    HAL_FLASH_Lock();
#endif
    return ret;
}

int8_t bsp_flash_erase(uint32_t addr,uint32_t len)
{
    if((0 == len)
    || (addr < BSP_FLASH_START))
    {
        return -BSP_FLASH_ERROR_PAREMETER;
    }
    else if(addr+len > ADDR_FLASH_END)
    {
        return -BSP_FLASH_ERROR_OVERRANGE;
    }
    else if(addr % BSP_FLASH_PAGE_SIZE)
    {
        return -BSP_FLASH_ERROR_ERASE_ALIGN;
    }
    uint32_t page_num = (len + BSP_FLASH_PAGE_SIZE - 1)/BSP_FLASH_PAGE_SIZE;
    return _bsp_flash_page_erase(addr,page_num);
}

int8_t bsp_flash_write(uint32_t addr,uint8_t *data,uint32_t len)
{
    if((0 == len)
    || (addr < BSP_FLASH_START))
    {
        return -BSP_FLASH_ERROR_PAREMETER;
    }
    else if(addr+len > ADDR_FLASH_END)
    {
        return -BSP_FLASH_ERROR_OVERRANGE;
    }
    return _bsp_flash_write(addr,data,len);
}

int8_t bsp_flash_read(uint32_t addr,uint8_t *data,uint32_t len)
{
    if((0 == len)
    || (addr < BSP_FLASH_START))
    {
        return -BSP_FLASH_ERROR_PAREMETER;
    }
    else if(addr+len > ADDR_FLASH_END)
    {
        return -BSP_FLASH_ERROR_OVERRANGE;
    }
    memcpy(data,(uint8_t*)addr,len);
    return BSP_FLASH_ERROR_NULL;
}

int8_t bsp_flash_erase_and_write(uint32_t addr,uint8_t *data,uint32_t len,uint8_t is_end_keep)
{
    if((0 == len)
    || (addr < BSP_FLASH_START))
    {
        return -BSP_FLASH_ERROR_PAREMETER;
    }
    else if(addr+len > ADDR_FLASH_END)
    {
        return -BSP_FLASH_ERROR_OVERRANGE;
    }
    int8_t ret = 0;
    volatile uint32_t erase_addr = (addr / BSP_FLASH_PAGE_SIZE)*BSP_FLASH_PAGE_SIZE;
    volatile uint32_t read_len = addr - erase_addr;
    volatile uint32_t write_len = len + read_len;
    volatile uint32_t data_index = 0;
    if(read_len)
    {
        HAL_IWDG_Refresh(&hiwdg);
        uint32_t len_temp = write_len > BSP_FLASH_PAGE_SIZE ? BSP_FLASH_PAGE_SIZE:write_len;
        memcpy(flash_read_temp,(uint8_t*)erase_addr,read_len);
        memcpy(&flash_read_temp[read_len],data,len_temp-read_len);
        data_index = len_temp-read_len;
        ret = _bsp_flash_page_erase(erase_addr,1);
        if(ret)
        {
            return ret;
        }
        __DSB();
        __ISB();
        ret =  _bsp_flash_write(erase_addr,flash_read_temp,len_temp);
        if(ret)
        {
            return ret;
        }   
        erase_addr += len_temp;
        write_len -= len_temp;
    }

    while(write_len > BSP_FLASH_PAGE_SIZE)
    {
        HAL_IWDG_Refresh(&hiwdg);
        ret = _bsp_flash_page_erase(erase_addr,1);
        if(ret)
        {
            return ret;
        }
        __DSB();
        __ISB();
        ret =  _bsp_flash_write(erase_addr,&data[data_index],BSP_FLASH_PAGE_SIZE); 
        if(ret)
        {
            return ret;
        }  
        erase_addr += BSP_FLASH_PAGE_SIZE;
        write_len -= BSP_FLASH_PAGE_SIZE;
        data_index += BSP_FLASH_PAGE_SIZE;
    }

    if(write_len)
    {
        HAL_IWDG_Refresh(&hiwdg);
        if(is_end_keep)
        {
            memcpy(flash_read_temp,&data[data_index],write_len);
            memcpy(&flash_read_temp[write_len],(uint8_t*)(erase_addr + write_len),BSP_FLASH_PAGE_SIZE-write_len);
            ret = _bsp_flash_page_erase(erase_addr,1);
            if(ret)
            {
                return ret;
            }
            __DSB();
            __ISB();
            ret =  _bsp_flash_write(erase_addr,flash_read_temp,BSP_FLASH_PAGE_SIZE); 
            if(ret)
            {
                return ret;
            }
        }
        else
        {
            ret = _bsp_flash_page_erase(erase_addr,1);
            if(ret)
            {
                return ret;
            }
            __DSB();
            __ISB();
            ret =  _bsp_flash_write(erase_addr,&data[data_index],write_len); 
            if(ret)
            {
                return ret;
            }
        }
    }
    return ret;
}

#ifdef BSP_FLASH_UNIT_TEST_EN

int8_t bsp_flash_test(const uint32_t addr,const uint32_t len)
{
    int8_t ret;
    if((0 == len)
    || (addr < BSP_FLASH_START))
    {
        return -BSP_FLASH_ERROR_PAREMETER;
    }
    else if(addr+len > ADDR_FLASH_END)
    {
        return -BSP_FLASH_ERROR_OVERRANGE;
    }
    for(uint16_t i = 0;i < BSP_FLASH_PAGE_SIZE;i++)
    {
        flash_write_temp[i] = i;
    }
    uint32_t write_addr = addr;
    uint32_t write_len = len;
    uint32_t write_tick_diff;
    uint32_t read_tick_diff;
    uint32_t start_tick = HAL_GetTick();
    while(write_len > BSP_FLASH_PAGE_SIZE)
    {
        ret = bsp_flash_erase_and_write(write_addr,flash_write_temp,BSP_FLASH_PAGE_SIZE,0);
        if(ret)
        {
            printf("erase and write failed 1:%d\n",ret);
        }
        write_addr += BSP_FLASH_PAGE_SIZE;
        write_len -= BSP_FLASH_PAGE_SIZE;
    }
    if(write_len)
    {
        ret = bsp_flash_erase_and_write(write_addr,flash_write_temp,write_len,0);
        if(ret)
        {
            printf("erase and write failed 2:%d\n",ret);
        }
    }
    write_tick_diff = HAL_GetTick() - start_tick;
    __DSB();
    __ISB();
    write_addr = addr;
    write_len = len;
    start_tick = HAL_GetTick();
    while(write_len > BSP_FLASH_PAGE_SIZE)
    {
        bsp_flash_read(write_addr,flash_read_temp,BSP_FLASH_PAGE_SIZE);
        for(uint16_t i = 0;i < BSP_FLASH_PAGE_SIZE;i++)
        {
            if(flash_write_temp[i] != flash_read_temp[i])
            {
                printf("check failed:%lx,%d,%d\n",write_addr+i,flash_write_temp[i],flash_read_temp[i]);
                return -BSP_FLASH_ERROR_TEST_CHECK;
            }
        }
        write_addr += BSP_FLASH_PAGE_SIZE;
        write_len -= BSP_FLASH_PAGE_SIZE;
    }
    if(write_len)
    {
        bsp_flash_read(write_addr,flash_read_temp,write_len);
        for(uint16_t i = 0;i < write_len;i++)
        {
            if(flash_write_temp[i] != flash_read_temp[i])
            {
                printf("check failed:%lx,%d,%d\n",write_addr+i,flash_write_temp[i],flash_read_temp[i]);
                return -BSP_FLASH_ERROR_TEST_CHECK;
            }
        }
    }
    read_tick_diff = HAL_GetTick() - start_tick;
    write_tick_diff = write_tick_diff ? write_tick_diff:1;
    read_tick_diff = read_tick_diff ? read_tick_diff:1;
    printf("write_speed:%ldKB/s,read_speed:%ldKB/s\n",len/write_tick_diff,len/read_tick_diff);
    return BSP_FLASH_ERROR_NULL;
}
typedef struct 
{
    uint32_t addr;
    uint32_t len;
}FlashUnitTest_st;

void bsp_flash_unit_test(void)
{
    int8_t ret;
#ifdef BSP_FLASH_CONDITION_TEST
    const FlashUnitTest_st condition_test[] = 
    {
        {BSP_FLASH_START-1,0xFFF},
        {BSP_FLASH_START,0},
        {BSP_FLASH_START,0xFFF},
        {BSP_FLASH_START,0x1000},
        {BSP_FLASH_START,0x1001},
        {BSP_FLASH_START,0x4000},
        {BSP_FLASH_START,0x4001},
        {BSP_FLASH_START+1,0x4001},
        {BSP_FLASH_START,BSP_FLASH_SIZE},
        {BSP_FLASH_START,BSP_FLASH_SIZE+1},
        {BSP_FLASH_START+1,BSP_FLASH_SIZE-1},
    };

    for(uint8_t i = 0;i < sizeof(condition_test)/sizeof(FlashUnitTest_st);i++)
    {
        HAL_IWDG_Refresh(&hiwdg);
        ret = bsp_flash_sector_erase(condition_test[i].addr,condition_test[i].len);
        printf("FLASH_UNIT %d sector erase %d,%lx,%lx\n",i,ret,condition_test[i].addr,condition_test[i].len);
        ret = bsp_flash_erase(condition_test[i].addr,condition_test[i].len);
        printf("FLASH_UNIT %d erase %d,%lx,%lx\n",i,ret,condition_test[i].addr,condition_test[i].len);
    }
    
#endif
    #define TEST_ADDR                              0x08080000
    #define TEST_MAX                               0x70000
    const FlashUnitTest_st test_sector_erase[] = 
    {
        {TEST_ADDR,0x1FFFF},
        {TEST_ADDR-1,0x1FFFF},
        {TEST_ADDR,0},
        {TEST_ADDR,0x20000},
        {TEST_ADDR,0x20001},
        {TEST_ADDR+1,0x20001},
        {TEST_ADDR,TEST_MAX},
        {TEST_ADDR,TEST_MAX+1},
        {TEST_ADDR+1,TEST_MAX-1},
        {TEST_ADDR,0x80000},
        {TEST_ADDR,0x80000+1},
        {TEST_ADDR+1,0x80000-1},
    };

    for(uint8_t i = 0;i < sizeof(test_sector_erase)/sizeof(FlashUnitTest_st);i++)
    {
        HAL_IWDG_Refresh(&hiwdg);
        ret = bsp_flash_sector_erase(test_sector_erase[i].addr,test_sector_erase[i].len);
        printf("FLASH_UNIT %d sector erase %d,%lx,%lx\n",i,ret,test_sector_erase[i].addr,test_sector_erase[i].len);
        ret = bsp_flash_erase(test_sector_erase[i].addr,test_sector_erase[i].len);
        printf("FLASH_UNIT %d page erase %d,%lx,%lx\n",i,ret,test_sector_erase[i].addr,test_sector_erase[i].len);
    }

    const FlashUnitTest_st test_page_erase[] = 
    {
        {TEST_ADDR,0xFFF},
        {TEST_ADDR-1,0xFFF},
        {TEST_ADDR,0},
        {TEST_ADDR,0x1000},
        {TEST_ADDR,0xFFF},
        {TEST_ADDR+1,0xFFF},
    };
    
    for(uint8_t i = 0;i < sizeof(test_page_erase)/sizeof(FlashUnitTest_st);i++)
    {
        HAL_IWDG_Refresh(&hiwdg);
        ret = bsp_flash_erase(test_page_erase[i].addr,test_page_erase[i].len);
        printf("FLASH_UNIT %d erase %d,%lx,%lx\n",i,ret,test_page_erase[i].addr,test_page_erase[i].len);
        ret = bsp_flash_write(test_page_erase[i].addr,flash_write_temp,test_page_erase[i].len);
        printf("FLASH_UNIT %d write %d,%lx,%lx\n",i,ret,test_page_erase[i].addr,test_page_erase[i].len);
    }
}
#endif